#include <im_joint_state_publisher/im_joint_state_publisher.h>

namespace im_joint_state_publisher {

ImJointStatePublisher::ImJointStatePublisher() {
	server = new interactive_markers::InteractiveMarkerServer("im_joint_state_publisher_server", "", false);
	joint_states = node.advertise<sensor_msgs::JointState>("joint_states", 1);
	node.param<std::string>("tf_prefix",tf_prefix,"");
	timer.stop();
}
ImJointStatePublisher::~ImJointStatePublisher() {
	clear();
}
bool ImJointStatePublisher::loadNewRobot() {
	clear();
	if (!robot_model.initParam("robot_description")) {
		return false;
		ROS_INFO("No parameter found");
	}
	ROS_INFO("ROBOT FOUND");

	robot_model.getLinks(links_list);
	createJointMarker();

	timer = node.createTimer(ros::Duration(.1), &ImJointStatePublisher::updateJointsAndMarkers, this);
	timer.start();

	robot_name = robot_model.getName();
	root = robot_model.getRoot()->name;

	return true;
}

bool ImJointStatePublisher::loadNewRobot(const std::string filename) {
	clear();
	if (!robot_model.initFile(filename)) {
		return false;
		ROS_INFO("No file found");
	}
	ROS_INFO("ROBOT FOUND");

	robot_model.getLinks(links_list);
	createJointMarker();
	timer = node.createTimer(ros::Duration(.1), &ImJointStatePublisher::updateJointsAndMarkers, this);
	timer.start();

	robot_name = robot_model.getName();
	root = robot_model.getRoot()->name;

	return true;
}

bool ImJointStatePublisher::createJointMarker() {
	for (unsigned int i = 0; i < links_list.size(); i++) {
		if (links_list[i]->parent_joint == NULL)
			ROS_INFO("no parent for joint for link->%s", links_list[i]->name.c_str());
		else {
			if (!checkJoint(links_list[i]->parent_joint->name)) {
				double maxDimensionChild = 0.0, maxDimensionParent = 0.0, maxDimension = 0.0;
				const urdf::Link *parent = robot_model.getLink(links_list[i]->parent_joint->parent_link_name).get();
				if ((links_list[i]->visual.get() != NULL) && (links_list[i]->visual->geometry.get() != NULL)) {
					const urdf::Geometry* geo = links_list[i]->visual->geometry.get();
					maxDimensionChild = constructShape(geo);
				}
				ROS_DEBUG("Parent Dimension-> %f", maxDimensionChild);
				maxDimensionChild = std::max(0.05, maxDimensionChild);
				//ROS_DEBUG("Child Dimension-> %f",maxDimensionChild);

				if ((parent->visual.get() != NULL) && (parent->visual->geometry.get() != NULL)) {
					const urdf::Geometry* geo = parent->visual->geometry.get();
					maxDimensionParent = constructShape(geo);
				}
				ROS_DEBUG("Parent Dimension-> %f", maxDimensionParent);
				maxDimensionParent = std::max(0.05, maxDimensionParent);
				maxDimension = maxDimensionParent;

				if (maxDimensionChild > maxDimensionParent)
					maxDimension = maxDimensionChild;
				ROS_DEBUG("dimension %f", maxDimension);

				joint_list.push_back(im_joint_state_publisher::ImJoint(links_list[i]->parent_joint, server, maxDimension,
								boost::bind(&ImJointStatePublisher::processFeedback, this, _1)));
				joint[joint_list[count].joint_name] = count++;
			} else
				ROS_INFO("Joint already Exist");
		}
	}
	ROS_INFO("No. of Links=%d	Joints=%d", links_list.size(), joint_list.size());

	server->applyChanges();

	return true;
}

//bool ImJointStatePublisher::loadPreshape(const std::string filename) {
//	std::ifstream preshape_file;
//	preshape_file.open(filename.c_str());
//	if (preshape_file.is_open()) {
//		ROS_INFO("loading preshape from %s", filename.c_str());
//		std::string name;
//		preshape_file >> name;
//		if (name == robot_name) {
//			for (unsigned int i = 0; i < joint_list.size(); i++) {
//				preshape_file >> joint_list[i].joint_angle;
//				ROS_INFO(
//						"loaded %s with angle %f", joint_list[i].joint_name.c_str(), joint_list[i].joint_angle);
//			}
//		} else {
//			ROS_ERROR(
//					"file do not contain the loaded %s preshape", robot_name.c_str());
//			return false;
//		}
//		preshape_file.close();
//	} else {
//		ROS_ERROR("no file with this name %s ", filename.c_str());
//		return false;
//	}
//	return true;
//}
//
//bool ImJointStatePublisher::savePreshape(const std::string filename) {
//	ROS_INFO("Saving preshape to %s", filename.c_str());
//	std::ofstream preshape_file;
//	preshape_file.open(filename.c_str());
//	if (preshape_file.is_open()) {
//		preshape_file << robot_name << "\n";
//		for (unsigned int i = 0; i < joint_list.size(); i++) {
//			preshape_file << joint_list[i].joint_angle << "\n";
//			ROS_INFO(
//					"saved %s with angle %f", joint_list[i].joint_name.c_str(), joint_list[i].joint_angle);
//		}
//		preshape_file.close();
//	} else
//		return false;
//	return true;
//}

void ImJointStatePublisher::clear() {
	joint_list.clear();
	server->clear();
	links_list.clear();
	timer.stop();
	count = 0;
}

void ImJointStatePublisher::updateJointsAndMarkers(const ros::TimerEvent& e) {
	sensor_msgs::JointState joint_state;
	static tf::TransformBroadcaster br;
	std::vector<tf::StampedTransform> tf;
	tf::StampedTransform t;

	ros::Time time = ros::Time::now();
	unsigned int i = 0;
	for (i = 0; i < joint_list.size(); i++) {
		if ((joint_list[i].type != im_joint_state_publisher::ImJoint::FIXED) && (joint_list[i].mimic_name.size() == 0)) {
			std::string parent_frame = tf_prefix;
			std::string child_frame = tf_prefix;
			t.stamp_ = time;
			t.setOrigin(joint_list[i].origin);
			t.setRotation(joint_list[i].rotation);
			t.frame_id_ = parent_frame.append(joint_list[i].parent_name);
			t.child_frame_id_ = child_frame.append(joint_list[i].joint_name);
			tf.push_back(t);
		}
		if (joint_list[i].mimic_name.size() > 0) {
			int n = joint[joint_list[i].mimic_name];
			joint_list[i].joint_angle = joint_list[n].joint_angle * joint_list[i].multiplier + joint_list[i].offset;
			ROS_DEBUG("sending mimic %f", joint_list[i].joint_angle);
		}
		joint_state.name.push_back(joint_list[i].joint_name);
		joint_state.position.push_back(joint_list[i].joint_angle);
	}

	br.sendTransform(tf);
	joint_state.header.stamp = time;
	joint_states.publish(joint_state);
}

bool ImJointStatePublisher::checkLink(const std::string name) {
	for (unsigned int i = 0; i < links_list.size(); i += 1) {
		if (links_list[i]->name == name)
			return true;
	}
	return false;
}

bool ImJointStatePublisher::checkJoint(const std::string name) {
	for (unsigned int i = 0; i < joint_list.size(); i += 1) {
		if (joint_list[i].joint_name == name)
			return true;
	}
	return false;
}

void ImJointStatePublisher::processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) {
	joint_list[joint[feedback->marker_name]].setJointRotation(feedback);
}

//TODO: link to geometric_shapes on arm_navigation stack. Implement a computeMeshDimensions(shapes::Mesh) method
double ImJointStatePublisher::constructShape(const urdf::Geometry *geom) {
	ROS_ASSERT(geom);

	double result = .05;
	switch (geom->type) {
		case urdf::Geometry::SPHERE: {
			result = dynamic_cast<const urdf::Sphere*>(geom)->radius;
		}
		break;
		case urdf::Geometry::BOX: {
			urdf::Vector3 dim = dynamic_cast<const urdf::Box*>(geom)->dim;
			result = sqrt(dim.x * dim.x + dim.y * dim.y + dim.z * dim.z);
		}
		break;
		case urdf::Geometry::CYLINDER: {
			double radius = dynamic_cast<const urdf::Cylinder*>(geom)->radius;
			double height = dynamic_cast<const urdf::Cylinder*>(geom)->length;
			result = sqrt(radius * radius + height * height);
		}
		break;
		case urdf::Geometry::MESH: {
			const urdf::Mesh *mesh = dynamic_cast<const urdf::Mesh*>(geom);
			if (!mesh->filename.empty()) {
				tf::Vector3 scale(mesh->scale.x, mesh->scale.y, mesh->scale.z);
				result = createMeshFromFilename(mesh->filename, &scale);
			}
		}
		break;
		default:
			ROS_ERROR("Unknown geometry type: %d", (int)geom->type);
			break;
	}

	return result;
}

//TODO: link to geometric_shapes on arm_navigation stack. Implement a computeMeshDimensions(shapes::Mesh) method
//Then remove dependencies to assimp library
double ImJointStatePublisher::createMeshFromFilename(const std::string& filename, const tf::Vector3* scale) {
	resource_retriever::Retriever retriever;
	resource_retriever::MemoryResource res;

	try {
		res = retriever.get(filename);
	} catch (resource_retriever::Exception& e) {
		ROS_ERROR("%s", e.what());
		return .05;
	}
	if (res.size == 0) {
		ROS_WARN("Retrieved empty mesh for resource '%s'", filename.c_str());
		return .05;
	}

	Assimp::Importer importer;
	std::string hint;
	std::size_t pos = filename.find_last_of(".");
	if (pos != std::string::npos) {
		hint = filename.substr(pos + 1);
		if (hint.find("stl") != std::string::npos)
			hint = "stl";
	}
	const aiScene* scene = importer.ReadFileFromMemory(
			reinterpret_cast<void*>(res.data.get()),
			res.size,
			aiProcess_Triangulate | aiProcess_JoinIdenticalVertices | aiProcess_SortByPType, hint.c_str());
	if (!scene) {
		ROS_WARN_STREAM("Assimp reports no scene in " << filename);
		return .05;
	}
	if (!scene->HasMeshes()) {
		ROS_WARN_STREAM("Assimp reports scene in " << filename << " has no meshes");
		return .05;
	}
	if (scene->mNumMeshes > 1) {
		ROS_WARN_STREAM("Mesh loaded from " << filename << " has " << scene->mNumMeshes << " but only the first one will be used");
	}

	aiNode *node1 = scene->mRootNode;

	bool found = false;
	if (node1->mNumMeshes > 0 && node1->mMeshes != NULL) {
		ROS_DEBUG_STREAM("Root node has meshes " << (int)node1->mMeshes);
		found = true;
	} else {
		for (uint32_t i = 0; i < node1->mNumChildren; ++i) {
			if (node1->mChildren[i]->mNumMeshes > 0 && node1->mChildren[i]->mMeshes != NULL) {
				ROS_DEBUG_STREAM("Child " << i << " has meshes");
				node1 = node1->mChildren[i];
				found = true;
				break;
			}
		}
	}
	if (found == false) {
		ROS_WARN_STREAM("Can't find meshes in file");
		return .05;
	}

	aiMatrix4x4 transform = node1->mTransformation;
	tf::Vector3 ts(1.0, 1.0, 1.0);
	if (scale != NULL) {
		ts = (*scale);
	}
	ROS_DEBUG("%f %f %f", ts.x(), ts.y(), ts.z());

	return (createMeshFromAsset(scene->mMeshes[node1->mMeshes[0]], transform, ts));
}

//TODO: link to geometric_shapes on arm_navigation stack. Implement a computeMeshDimensions(shapes::Mesh) method
double ImJointStatePublisher::createMeshFromAsset(const aiMesh* a, const aiMatrix4x4& transform, const tf::Vector3& scale) {
	if (!a->HasFaces()) {
		ROS_ERROR("Mesh asset has no faces");
		return .05;
	}

	if (!a->HasPositions()) {
		ROS_ERROR("Mesh asset has no positions");
		return .05;
	}

	for (unsigned int i = 0; i < a->mNumFaces; ++i) {
		if (a->mFaces[i].mNumIndices != 3) {
			ROS_ERROR("Asset is not a triangle mesh");
			return .05;
		}
	}
	ROS_DEBUG("Vertices->%d Faces->%d", a->mNumVertices, a->mNumFaces);

	double maxDimension = 0.0;
	for (unsigned int i = 0; i < a->mNumVertices; ++i) {
		aiVector3D p;
		p.x = a->mVertices[i].x;
		p.y = a->mVertices[i].y;
		p.z = a->mVertices[i].z;
		p *= transform;
		double x = p.x * scale.x();
		double y = p.y * scale.x();
		double z = p.z * scale.x();
		if (maxDimension < sqrt(x * x + y * y + z * z)) {
			maxDimension = sqrt(x * x + y * y + z * z);
			//			ROS_DEBUG("Dimension changed to %f",maxDimension);
		}
	}
	return maxDimension;
}
}
// namespace im_joint_state_publisher
